/* 
 * File:   Calculate.cpp
 * Author: song
 * 
 * Created on 2010年11月21日, 上午9:40
 */

#include <fstream>

#include "Calculate.h"
extern int numFrame;

Calculate::Calculate()
{
}

Calculate::Calculate(Camera *cam, trackObject *objsL, trackObject *objsR, int size = 10) {
    this->otherCam = cam;
    this->objs.L = objsL;
    this->objs.R = objsR;
    this->objSize = size;
    this->f = 825;
    this->tx = 18;

    //距离摄像头一米时，30厘米对应了260像素
    this->z = 100;
    this->pixDis = 30;
    this->pixNum = 260;
    this->factor = 1.0 * pixDis/pixNum /z;

    float hranges_arr[]={0,180};
    float *hranges = hranges_arr;
    int hdims = 256;
    tmpobj.hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
    tmpobj.image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
    tmpobj.mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);
    tmpobj.hsv = cvCreateImage( cvGetSize(cam->pFrame), 8, 3 );
    tmpobj.hue = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
    tmpobj.backProject = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );

    for(int i = 0; i < 20; i++) {
        objs.objCal[i].match = false;
        objs.objCal[i].tracked = false;
    }

    fout.open("speed.txt");
}

Calculate::Calculate(const Calculate& orig) {
}

Calculate::~Calculate() {
}

bool cmp(const trackObject &a, const trackObject &b)
{
    int ya = a.rect.y + a.rect.height/2;
    int yb = b.rect.y + b.rect.height/2;
    int t = ya - yb;
    if(t < 0) t = -t;
    if(t < 10) {
        ya = a.rect.x + a.rect.height/2;
        yb = b.rect.x + b.rect.height/2;
    }
    return ya < yb;
}

void Calculate::Cal()
{
    int i;
    bool lFlag[20] = {0}, rFlag[20] = {0};

    //判断是否有匹配关系不成立
    for(i = 0; i < objSize; i++)
    {
        if(objs.objCal[i].match == true) {
            if(objs.L[objs.objCal[i].lIndex].deleted == false && 
                    objs.R[objs.objCal[i].rIndex].deleted == false &&
                    objs.L[objs.objCal[i].lIndex].id == objs.objCal[i].lID && 
                    objs.objCal[i].rID == objs.R[objs.objCal[i].rIndex].id){
                objs.objCal[i].match = true;
                lFlag[objs.objCal[i].lIndex] = true;
                rFlag[objs.objCal[i].rIndex] = true;

                //调试代码，输出匹配的物体信息
//                cout<<"keep match index: "<<i<<" left index: "<<objs.objCal[i].lIndex
//                        <<"  right index: "<<objs.objCal[i].rIndex<<
//                        "  with: "<<objs.L[i].pt1.x - objs.L[i].pt0.x<<
//                        "  height: "<<objs.L[i].pt1.x - objs.L[i].pt0.x<<endl;
            }
            else
                objs.objCal[i].match = false;
        }
    }

    //将没有追踪中的物体加入匹配数组
    objs.lLen = objs.rLen = 0;
    for(i = 0; i < this->objSize; i++) {
        if(lFlag[i] == false && objs.L[i].deleted == false)
        {
            objs.objsL[objs.lLen].showID = i;
            objs.objsL[objs.lLen++] = objs.L[i];
            objs.lIndex[objs.lLen-1] = i;

            //调试代码，输出进入匹配数组的物体信息
//            cout<<"left index: "<<i<<" x:"<<objs.L[i].pt0.x<<" y:"<<objs.L[i].pt0.y<<
//                    "  with: "<<objs.L[i].pt1.x - objs.L[i].pt0.x<<
//                    "  height: "<<objs.L[i].pt1.x - objs.L[i].pt0.x<<endl;

        }
        if(rFlag[i] == false && objs.R[i].deleted == false)
        {
            objs.objsR[objs.rLen].showID = i;
            objs.objsR[objs.rLen++] = objs.R[i];
            objs.rIndex[objs.rLen - 1] = i;

            //调试代码，输出进入匹配数组的物体信息
//            cout<<"right index: "<<i<<" x:"<<objs.R[i].pt0.x<<" y:"<<objs.R[i].pt0.y<<
//                    "  with: "<<objs.R[i].pt1.x - objs.R[i].pt0.x<<
//                    "  height: "<<objs.R[i].pt1.x - objs.R[i].pt0.x<<endl;
        }
    }
       

    //两组物体进行匹配
    Match(objs.objsL, objs.objsR);

    //计算当前距离，x，y，z
    CalZDis();

    //计算个方向速度
    //CalSpeed();

}

//参数计算模块的预处理部分，进行两幅画面中物体匹配和适别
//次函数应该只处理未被追踪中的物体
void Calculate::Match(trackObject objsL[], trackObject objsR[])
{
    //先按照y轴坐标差排序，y相差在10以内则进行x排序
    //待修改，数据结构不对

    sort(objsL, objsL+objs.lLen, cmp);
    sort(objsR, objsR+objs.rLen, cmp);

    int i, j, k;
    bool flag[20] = {0};

    for(i = 0; i < objs.lLen; i++) {
        objs.objCal[objsL[i].showID].match = false;

        if(objsL[i].deleted == false) {
            for(j = 0; j < objs.rLen; j++) {
                //以左幅图像为准一一进行匹配
                if(flag[j] == false) {
                    if(CalD(objsL[i].hist, objsR[j].hist) == true)
                    {
                        //统一用左视图的编号作为显示,showID一开始为原数组下标
                        objs.objCal[objsL[i].showID].lIndex = objs.lIndex[i];
                        objs.objCal[objsL[i].showID].rIndex = objs.rIndex[j];
                        //objsR[j].showID = objsL[i].showID;
                        objs.R[j].showID = objsL[i].showID;

                        objs.objCal[objsL[i].showID].match = true;
                        objs.objCal[objsL[i].showID].lID = objsL[i].id;
                        objs.objCal[objsL[i].showID].rID = objsR[j].id;
                        objs.objCal[objsL[i].showID].tracked = false;
                        objs.objCal[objsL[i].showID].Vnum = 0;

                        flag[j] = true;
                        break;
                    }
                }
            }
        }
    }
}

//通过camshift进行两幅图像的匹配
void Calculate::Match()
{
    int i;
    trackObject *obj = &tmpobj;
    
    for(i = 0; i < this->objSize; i++) {
        objs.objCal[i].match = 0;
        if(objs.objsL[i].deleted == false) {
            *obj = objs.objsL[i];
            //在另外一个帧中计算camshift
            
            cvCopy(otherCam->pFrame, obj->image, 0);
            cvCvtColor(obj->image, obj->hsv, CV_BGR2HSV);
            cvInRangeS(obj->hsv, cvScalar(0, 30, 10, 0),
                    cvScalar(180, 256, 256,0), obj->mask );
            cvSplit(obj->hsv, obj->hue, 0, 0, 0);

            //设置图像的感兴趣区域
            cvSetImageROI(obj->hue, obj->rect);
            cvSetImageROI(obj->mask, obj->rect);

            cvCalcHist(&obj->hue, obj->hist, 0, obj->mask);
            float max_val = 0.0;
            cvGetMinMaxHistValue(obj->hist, 0, &max_val, 0, 0 );
            cvConvertScale(obj->hist->bins, obj->hist->bins,
                    max_val ? 255. / max_val : 0, 0 );
            cvResetImageROI(obj->hue);
            cvResetImageROI(obj->mask);
            //计算目标直方图的反向投影
            cvCalcBackProject(&(obj->hue), obj->backProject, obj->hist);

            //蒙板
            cvAnd(obj->backProject, obj->mask, obj->backProject, 0);
            //执行CamShift算法跟踪目标
            cvCamShift(obj->backProject, obj->rect,
                    cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                    &obj->comp, &obj->box);
            
            //判断是否丢失
                if(obj->comp.rect.width <= 0 || obj->comp.rect.height <= 0
            || obj->box.size.width <= 0 || obj->box.size.height <= 0
            || obj->box.center.x < 0 || obj->box.center.x >= otherCam->pFrame->width
            || obj->box.center.y < 0 || obj->box.center.y >= otherCam->pFrame->height)
                {
                    objs.objCal[i].match = false;
                    continue;
                }
            
            //判断是否匹配错误，通过简单面积的变化率
            int s1 = objs.objsL[i].comp.rect.height * objs.objsL[i].comp.rect.width;
            int s2 = obj->comp.rect.height * obj->comp.rect.width;
            int dis = s1-s2;
            if(dis < 0) dis = -dis;
            float var = (1.0*dis) / s1;
            
            if(var > 1.5 || var < 0.5 ) objs.objCal[i].match = false;
            else {
                objs.objCal[i].match = true;
                objs.objCal[i].dx = obj->comp.rect.x - objs.objsL[i].comp.rect.x;
                if(objs.objCal[i].dx < 0) objs.objCal[i].dx = -objs.objCal[i].dx;
                CvPoint pt0, pt1;
                pt0.x = obj->comp.rect.x;
                pt0.y = obj->comp.rect.y;
                pt1.x = pt0.x + obj->comp.rect.width;
                pt1.y = pt0.y + obj->comp.rect.height;
                
                cvRectangle(otherCam->pFrame, pt0, pt1, CV_RGB(255,0,0), 2);
            }

        }
    }
}

//通过计算出的像素差计算视深
void Calculate::CalZDis()
{
    int i;
    char s [100];
    CvFont * font = &cvFont(2);
    CvPoint center0, center1, centerFrame;
    centerFrame.x = this->otherCam->pFrame->width / 2;
    centerFrame.y = this->otherCam->pFrame->height / 2;

    for(i = 0 ; i < this->objSize; i++) {
        if( objs.objCal[i].match == true) {
            //匹配成功后计算视差
            center0.x = (objs.L[objs.objCal[i].lIndex].pt0.x + objs.L[objs.objCal[i].lIndex].pt1.x) / 2;
            center0.y = (objs.L[objs.objCal[i].lIndex].pt0.y + objs.L[objs.objCal[i].lIndex].pt1.y) / 2;
            center1.x = (objs.R[objs.objCal[i].rIndex].pt0.x + objs.R[objs.objCal[i].rIndex].pt1.x) / 2;
            center1.y = (objs.R[objs.objCal[i].rIndex].pt0.y + objs.R[objs.objCal[i].rIndex].pt1.y) / 2;

            //计算各个方向的位置，以左图像的中心点为坐标系中心
            //待修该，视差应该由locate划分的中心进行计算，也许效果会好一些
            if(center0.x - center1.x == 0) {
                if(objs.objCal[i].tracked == false){
                    objs.objCal[i].match = true;
                    continue;
                }
                center0.x += objs.objCal[i].dx;
            }
            //范围控制在15米以内，对应的视差不能小于10，当小于时根据上一帧的视深距离比例放缩
            int preDx = objs.objCal[i].dx;
            int xDis = center0.x - center1.x;
            //引入前景捕捉部分最大面积图像的中心点进行修正，取视差最大的为准
            if(objs.L[objs.objCal[i].lIndex].locateSize != -1 && objs.R[objs.objCal[i].rIndex].locateSize != -1){
                if(objs.L[objs.objCal[i].lIndex].locateX - objs.R[objs.objCal[i].rIndex].locateX > xDis)
                    xDis = objs.L[objs.objCal[i].lIndex].locateX - objs.R[objs.objCal[i].rIndex].locateX;
            }
            objs.objCal[i].dx = center0.x - center1.x;
            if(objs.objCal[i].dx < 0)
                objs.objCal[i].dx = -objs.objCal[i].dx;
            if(objs.objCal[i].dx < 10){
//                int t = preDx - preDx/4;
//                if(t < 10) t = 10+preDx%10;
                if(objs.objCal[i].dx < 6)
                    objs.objCal[i].dx *= 4;
                else objs.objCal[i].dx += 10;

                if(objs.objCal[i].dx < 10)
                    objs.objCal[i].dx += 10;
            }

            //控制视深变化
            int preZDis = objs.objCal[i].zDis;
            objs.objCal[i].zDis = (this->f*this->tx)/objs.objCal[i].dx;
            int nowZDis = objs.objCal[i].zDis;
            //限制物体速度上限为10米每秒
            if(objs.objCal[i].tracked == true){
                double dis = (preZDis - nowZDis)/preZDis;
                if( preZDis-nowZDis > 100){
                   nowZDis = preZDis - 100 + dis*100; 
                }
                else if(preZDis-nowZDis < -1000)
                {
                    dis = -dis;
                    nowZDis = preZDis + 100 - dis*100;
                }
            }

            objs.objCal[i].zDis = nowZDis;
            objs.objCal[i].xDis = (center0.x - centerFrame.x)*factor*objs.objCal[i].zDis;
            objs.objCal[i].yDis = (center0.y - centerFrame.y)*factor*objs.objCal[i].zDis;
            if(objs.objCal[i].tracked == false)
            {
                objs.objCal[i].tracked = true;
                objs.objCal[i].num = 0;
            }
            
            int tLen = objs.objCal[i].num;

            //存储各个方向的历史运动信息
            if(objs.objCal[i].num >= NUMSPEEDFRAME)
            {
                for(int j = 0; j < NUMSPEEDFRAME -1 ; j++) {
                    objs.objCal[i].time[j] = objs.objCal[i].time[j+1];
                    objs.objCal[i].preX[j] = objs.objCal[i].preX[j+1];
                    objs.objCal[i].preY[j] = objs.objCal[i].preY[j+1];
                    objs.objCal[i].preZ[j] = objs.objCal[i].preZ[j+1];
                }
                objs.objCal[i].num--;
            }
            objs.objCal[i].time[objs.objCal[i].num] = clock();
            objs.objCal[i].preX[objs.objCal[i].num] = objs.objCal[i].xDis;
            objs.objCal[i].preY[objs.objCal[i].num] = objs.objCal[i].yDis;
            objs.objCal[i].preZ[objs.objCal[i].num] = objs.objCal[i].zDis;
            
            if(objs.objCal[i].num < NUMSPEEDFRAME)
                objs.objCal[i].num++;

            //修正XYZ坐标,计算速度
            FixCord(objs.objCal[i].xDis, objs.objCal[i].fixX, tLen);
            FixCord(objs.objCal[i].yDis, objs.objCal[i].fixY, tLen);
            FixCord(objs.objCal[i].zDis, objs.objCal[i].fixZ, tLen);
            if(i == 0)
                cout<<"Vnum 0: "<<objs.objCal[i].Vnum<<endl;
            FixSpeed(objs.objCal[i]);


            cout<<"objct:" + i<<"   ZDis "<<objs.objCal[i].zDis<<"  XDis "<<
                    objs.objCal[i].xDis << "    YDis "<<objs.objCal[i].yDis<<endl;
            cout<<"dx: "<<objs.objCal[i].dx<<endl;
            sprintf(s, "%d", objs.objCal[i].zDis);
            CvPoint tmp = objs.R[objs.objCal[i].rIndex].pt1;
            tmp.y = objs.R[objs.objCal[i].rIndex].pt0.y;
            cvPutText(otherCam->pFrame, s, tmp, font, CV_RGB(255,0,0));
        }
    }
}

//修正坐标数据
void Calculate::FixCord(float pre, float fix[], int len)
{
    int fixLen = len;
    if(fixLen < 0) return;
    //首先修正最后一个位置信息
    if(fixLen > 1)
        fix[fixLen-1] = (fix[fixLen - 2] + pre  + 2*fix[fixLen - 1]) / 4;

    //入对列的时候判断是否队列为满
    if(fixLen >= NUMSPEEDFRAME)
    {
        for(int i = 0; i < NUMSPEEDFRAME-1; i++)
            fix[i] = fix[i+1];
        fixLen--;
    }

    //入队列
    fix[fixLen] = pre;
}

void Calculate::FixSpeed(parmNode &obj)
{
    if(obj.num < 3) return;
    int n = obj.num-1;
    double dt = (obj.time[n-1] - obj.time[n - 2])*1.0/CLOCKS_PER_SEC;
    obj.Vx[obj.Vnum] = (obj.fixX[n-1] - obj.fixX[n - 2]) / dt;
    obj.Vy[obj.Vnum] = (obj.fixY[n-1] - obj.fixY[n - 2]) / dt;
    obj.Vz[obj.Vnum] = (obj.fixZ[n-1] - obj.fixZ[n - 2]) / dt;
    obj.Vnum++;
}

vector <speedNode> Calculate::GetSpeed()
{
    vector <speedNode> Vs;
    speedNode tmp;
    for(int i = 0; i < objSize; ++i)
    {
        if(!objs.objCal[i].match) continue;
        tmp.Vx = tmp.Vy = tmp.Vz = 0;
        int n = objs.objCal[i].Vnum;
        double t = n * (n + 1) * (n * 2 + 1) / 6.0;
        for(int j = 0; j < n; ++j)
        {
            tmp.Vx += objs.objCal[i].Vx[j] * (j + 1) * (j + 1) / t;
            tmp.Vy += objs.objCal[i].Vy[j] * (j + 1) * (j + 1) / t ;
            tmp.Vz += objs.objCal[i].Vz[j] * (j + 1) * (j + 1) / t ;
        }
        objs.objCal[i].Vnum = 0;
        tmp.id = i;
        //也应该做个加权
        tmp.zDis = objs.objCal[i].zDis;
        Vs.push_back(tmp);
    }
    return Vs;
}

//计算各个方向的速度函数
void Calculate::CalSpeed()
{
    int i, k;
    double time;

    fout<<"Frame:"<<numFrame<<endl;
    for(i = 0; i < objSize; i++) {
        if(objs.objCal[i].match == true && objs.objCal[i].num > 1) {

            int num=objs.objCal[i].num;
            //double tmp=(num-1)*(num)*(2*num-1)/6.0;
            double tmp=num*(num-1)/2.0;
            double xspeed=0.0,yspeed=0.0,zspeed=0.0;
            for(int k=1;k<=num-1;k++)
            {
                double t=(objs.objCal[i].time[k]-objs.objCal[i].time[k-1])*1.0/CLOCKS_PER_SEC;
                xspeed+=(objs.objCal[i].preX[k]-objs.objCal[i].preX[k-1])/t*(k/tmp);
                yspeed+=(objs.objCal[i].preY[k]-objs.objCal[i].preY[k-1])/t*(k/tmp);
                zspeed+=(objs.objCal[i].preZ[k]-objs.objCal[i].preZ[k-1])/t*(k/tmp);
            }

            objs.objCal[i].xSpeed=xspeed;
            objs.objCal[i].ySpeed=yspeed;
            objs.objCal[i].zSpeed=zspeed;

            /*
            time = (objs.objCal[i].time[objs.objCal[i].num-1] - objs.objCal[i].time[0])*1.0/CLOCKS_PER_SEC;
            
            objs.objCal[i].zSpeed = (objs.objCal[i].preZ[objs.objCal[i].num-1] - objs.objCal[i].preZ[0])*1.0
                    / time;
            objs.objCal[i].xSpeed = (objs.objCal[i].preX[objs.objCal[i].num-1] - objs.objCal[i].preX[0])*1.0
                    / time;
            objs.objCal[i].ySpeed = (objs.objCal[i].preY[objs.objCal[i].num-1] - objs.objCal[i].preY[0])*1.0
                    / time;
            */
            fout<<"objID: "<<i<<endl;
            //fout<<"caltime = "<<time<<endl;
            fout<<"xSpeed:" <<objs.objCal[i].xSpeed/100<<endl;
            fout<<"ySpeed:" <<objs.objCal[i].ySpeed/100<<endl;
            fout<<"zSpeed:" <<objs.objCal[i].zSpeed/100<<endl;
            fout<<endl;
        }
    }
}
//计算直方图方差，用来进行物体的匹配
bool Calculate::CalD(CvHistogram* hist0, CvHistogram* hist1)
{
    int h_bins = 256;
    //将统计的像素个数统计到0到MAX_Hist之内
    int MAX_Hist = 1000;

    double maps[2][300], max0 = 0, max1 = 0, mid0 = 0, mid1 = 0;
    int i, j, k;

    for(i = 0; i < h_bins; i++) {
        maps[0][i] = cvGetReal1D(hist0->bins,i);
        if(maps[0][i] > max0) max0 = maps[0][i];

        maps[1][i] = cvGetReal1D(hist1->bins,i);
        if(maps[1][i] > max1) max1 = maps[0][i];
    }

    //根据最大值统一到0到 MAX_Hist的范围内，如果最大值小于MAX_Hist则不变
    if(max0 > MAX_Hist){
        for(i = 0; i < h_bins; i++) {
            maps[0][i] = maps[0][i]/max0 * MAX_Hist;
            mid0 += maps[0][i];
        }
    }

    if(max1 > MAX_Hist){
        for(i = 0; i < h_bins; i++) {
            maps[1][i] = maps[1][i]/max1 * MAX_Hist;
            mid1 += maps[1][i];
        }
    }

    mid0 /= h_bins;
    mid1 /= h_bins;

    double variance[300] = {0}, mid = 0, dis = 0;
    for(i = 0; i < h_bins; i++) {
        variance[i] = maps[0][i] - maps[1][i];
        if(variance[i] < 0) variance[i] = -variance[i];
        mid += variance[i];
    }

    mid /= h_bins;
    for(i = 0; i < h_bins; i++) {
        dis += (mid - variance[i]) * (mid - variance[i]);
    }
//    cout<<"方差为:"<<dis<<endl;

    //认为小于MAX_Hist的平方为匹配成功
    if(dis < MAX_Hist * MAX_Hist)
        return true;
    return false;
}

void Calculate::Draw(IplImage* image, CvScalar color)
{
    char s[8];

    CvFont * font = &cvFont(2);
//    for(int i = 0; i < objSize; ++i)
//    {
//        if(objs.R[i].deleted) continue;
//       // printf("id:%d\tlostfrm:%d\t%d %d\n", V[i].id, V[i].lostfrm, V[i].pt1.x, V[i].pt1.y);
//        cvRectangle(image, objs.R[i].pt0, objs.R[i].pt1, color, 2);
//    }

    for(int i = 0; i < objSize; i++){
        if(objs.objCal[i].match == true)
        {
            cvRectangle(image, objs.R[objs.objCal[i].rIndex].pt0, objs.R[objs.objCal[i].rIndex].pt1, color, 2);
            sprintf(s, "%d", i);
            cvPutText(image, s, objs.R[objs.objCal[i].rIndex].pt1, font, color);
//            cout<<"right draw id: "<<i<<" rShowID: "<<objs.R[objs.objCal[i].rIndex].showID<<endl;
        }
    }
}